/*
 * Module   : SSD1289.c
 *
 * Created  : 22.01.2014 21:39:50
 * Author   : T & A
 */

#include "SSD1289.h"
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include <util/delay.h>
#include <string.h>

#define true  1
#define false 0

#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))

uint8_t fcolorr, fcolorg, fcolorb;
uint8_t bcolorr, bcolorg, bcolorb;
uint8_t orient;

long disp_y_size;
//volatile uint8_t *P_RS, *P_WR, *P_CS, *P_RST;
//uint8_t B_RS, B_WR, B_CS, B_RST;
_current_font	cfont;

/*
 * Prototypes
 */
void ssd1289_writeBus(char VH,char VL);
void ssd1289_writeCommand(char VL);
void ssd1289_writeData(char VH,char VL);
void ssd1289_writeCommandAndData(char com1,int dat1);
void ssd1289_setPixel(uint8_t r,uint8_t g,uint8_t b);
void ssd1289_drawHLine(int x, int y, int l);
void ssd1289_drawVLine(int x, int y, int l);
void ssd1289_printChar(uint8_t c, int x, int y);
void ssd1289_setXY(uint32_t x1, uint32_t y1, uint32_t x2, uint32_t y2);
void ssd1289_rotateChar(uint8_t c, int x, int y, int pos, int deg);


void ssd1289_writeBus(char VH,char VL) {
    
    PORTA = VH;
    PORTC = VL;

    cbi(PORTG, PG2);
    sbi(PORTG, PG2);
}

void ssd1289_writeCommand(char VL) {
    // RS
    cbi(PORTD, PD7);
    ssd1289_writeBus(0x00, VL);
}

void ssd1289_writeData(char VH, char VL) {
    // RS
    sbi(PORTD, PD7);
    ssd1289_writeBus(VH,VL);
}

void ssd1289_writeCommandAndData(char com1,int dat1) {
    ssd1289_writeCommand(com1);
    ssd1289_writeData(dat1 >> 8, dat1);
}

/* ssd1289_init() 
 * @orientation : landscape or portrait
 * @aspect : format (4:3 or 16:9)
 */
void ssd1289_init(uint8_t orientation, uint8_t aspect) {
    
    orient = orientation;
    
    DDRA = 0xFF;
    DDRC = 0xFF;
    DDRD = 0xFF;
    DDRG = 0xFF;
    

    // RS->PD7
    //sbi(PORTD, PD7);
    // WR->PG2
    //sbi(PORTG, PG2);
    // CS->PG1
    //sbi(PORTG, PG1);
    // RST->PG0
    //sbi(PORTG, PG0);

    switch (aspect) {
        case ASPECT_4x3:
        disp_y_size = 319;
        break;
        case ASPECT_16x9:
        disp_y_size = 399;
        break;
    }

    sbi(PORTG, PG0);
    //*P_RST |= B_RST;
    _delay_ms(5);
    cbi(PORTG, PG0);
    //*P_RST &= ~B_RST;
    _delay_ms(15);
    sbi(PORTG, PG0);
    //*P_RST |= B_RST;
    _delay_ms(15);

    if (disp_y_size == 319) {

        cbi(PORTG, PG1);
        //*P_CS &= ~B_CS;
        ssd1289_writeCommandAndData(0x0000,0x0001);
        _delay_ms(1); 
        ssd1289_writeCommandAndData(0x0003,0xA8A4);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x000C,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x000D,0x080C);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x000E,0x2B00);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x001E,0x00B7);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0001,0x2B3F);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0002,0x0600);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0010,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0011,0x6070);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0005,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0006,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0016,0xEF1C);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0017,0x0003);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0007,0x0233);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x000B,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x000F,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0041,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0042,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0048,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0049,0x013F);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x004A,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x004B,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0044,0xEF00);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0045,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0046,0x013F);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0030,0x0707);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0031,0x0204);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0032,0x0204);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0033,0x0502);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0034,0x0507);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0035,0x0204);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0036,0x0204);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0037,0x0502);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x003A,0x0302);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x003B,0x0302);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0023,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0024,0x0000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x0025,0x8000);
        _delay_ms(1);
        ssd1289_writeCommandAndData(0x004f,0);
        ssd1289_writeCommandAndData(0x004e,0);        
        ssd1289_writeCommand(0x22);
        sbi(PORTG, PG1);
        //*P_CS |= B_CS;

    } else {

        ssd1289_writeCommand(0xE9);
        ssd1289_writeData(0x00,0x20);
        ssd1289_writeCommand(0x11); //Exit Sleep
        _delay_ms(100);
        ssd1289_writeCommand(0xD1);
        ssd1289_writeData(0x00,0x00);
        ssd1289_writeData(0x00,0x71);
        ssd1289_writeData(0x00,0x19);
        ssd1289_writeCommand(0xD0);
        ssd1289_writeData(0x00,0x07);
        ssd1289_writeData(0x00,0x01);
        ssd1289_writeData(0x00,0x08);
        ssd1289_writeCommand(0x36);
        ssd1289_writeData(0x00,0x48);
        ssd1289_writeCommand(0x3A);
        ssd1289_writeData(0x00,0x05);
        ssd1289_writeCommand(0xC1);
        ssd1289_writeData(0x00,0x10);
        ssd1289_writeData(0x00,0x10);
        ssd1289_writeData(0x00,0x02);
        ssd1289_writeData(0x00,0x02);
        ssd1289_writeCommand(0xC0); //Set Default Gamma
        ssd1289_writeData(0x00,0x00);
        ssd1289_writeData(0x00,0x35);
        ssd1289_writeData(0x00,0x00);
        ssd1289_writeData(0x00,0x00);
        ssd1289_writeData(0x00,0x01);
        ssd1289_writeData(0x00,0x02);
        ssd1289_writeCommand(0xC5); //Set frame rate
        ssd1289_writeData(0x00,0x04);
        ssd1289_writeCommand(0xD2); //power setting
        ssd1289_writeData(0x00,0x01);
        ssd1289_writeData(0x00,0x44);
        ssd1289_writeCommand(0xC8); //Set Gamma
        ssd1289_writeData(0x00,0x04);
        ssd1289_writeData(0x00,0x67);
        ssd1289_writeData(0x00,0x35);
        ssd1289_writeData(0x00,0x04);
        ssd1289_writeData(0x00,0x08);
        ssd1289_writeData(0x00,0x06);
        ssd1289_writeData(0x00,0x24);
        ssd1289_writeData(0x00,0x01);
        ssd1289_writeData(0x00,0x37);
        ssd1289_writeData(0x00,0x40);
        ssd1289_writeData(0x00,0x03);
        ssd1289_writeData(0x00,0x10);
        ssd1289_writeData(0x00,0x08);
        ssd1289_writeData(0x00,0x80);
        ssd1289_writeData(0x00,0x00);
        ssd1289_writeCommand(0x2A);
        ssd1289_writeData(0x00,0x00);
        ssd1289_writeData(0x00,0x00);
        ssd1289_writeData(0x00,0x00);
        ssd1289_writeData(0x00,0xeF);
        ssd1289_writeCommand(0x2B);
        ssd1289_writeData(0x00,0x00);
        ssd1289_writeData(0x00,0x00);
        ssd1289_writeData(0x00,0x01);
        ssd1289_writeData(0x00,0x8F);
        ssd1289_writeCommand(0x29); //display on
        ssd1289_writeCommand(0x2C); //display on

    }

    ssd1289_setColor(255, 255, 255);
    ssd1289_setBackColor(0, 0, 0);
    cfont.font = 0;
}

void ssd1289_setXY(uint32_t x1, uint32_t y1, uint32_t x2, uint32_t y2) {
    int tmp;

    if (orient == LANDSCAPE) {
        tmp=x1;
        x1=y1;
        y1=disp_y_size-tmp;
        tmp=x2;
        x2=y2;
        y2=disp_y_size-tmp;
        tmp=y1;
        y1=y2;
        y2=tmp;
    }

    if (disp_y_size == 319) {

        ssd1289_writeCommandAndData(0x0044,(x2<<8)+x1);
        ssd1289_writeCommandAndData(0x0045,y1);
        ssd1289_writeCommandAndData(0x0046,y2);
        ssd1289_writeCommandAndData(0x004e,x1);
        ssd1289_writeCommandAndData(0x004f,y1);
        ssd1289_writeCommand(0x22);
        
    } else {
        
        ssd1289_writeCommand(0x2a);
        ssd1289_writeData(0x00,x1>>8);
        ssd1289_writeData(0x00,x1);
        ssd1289_writeData(0x00,x2>>8);
        ssd1289_writeData(0x00,x2);
        ssd1289_writeCommand(0x2b);
        ssd1289_writeData(0x00,y1>>8);
        ssd1289_writeData(0x00,y1);
        ssd1289_writeData(0x00,y2>>8);
        ssd1289_writeData(0x00,y2);
        ssd1289_writeCommand(0x2c);
    }
}

void ssd1289_drawRect(int x1, int y1, int x2, int y2) {
    int tmp;

    if (x1 > x2) {
        tmp = x1;
        x1 = x2;
        x2 = tmp;
    }
    
    if (y1 > y2) {
        tmp = y1;
        y1 = y2;
        y2 = tmp;
    }

    ssd1289_drawHLine(x1, y1, x2-x1);
    ssd1289_drawHLine(x1, y2, x2-x1);
    ssd1289_drawVLine(x1, y1, y2-y1);
    ssd1289_drawVLine(x2, y1, y2-y1);
}

void ssd1289_drawRoundRect(int x1, int y1, int x2, int y2) {
    int tmp;

    if (x1 > x2) {
        tmp = x1;
        x1 = x2;
        x2 = tmp;
    }
    
    if (y1 > y2) {
        tmp = y1;
        y1 = y2;
        y2 = tmp;
    }
    
    if ((x2-x1) > 4 && (y2-y1) > 4) {
        ssd1289_drawPixel(x1+1, y1+1);
        ssd1289_drawPixel(x2-1, y1+1);
        ssd1289_drawPixel(x1+1, y2-1);
        ssd1289_drawPixel(x2-1, y2-1);
        ssd1289_drawHLine(x1+2, y1, x2-x1-4);
        ssd1289_drawHLine(x1+2, y2, x2-x1-4);
        ssd1289_drawVLine(x1, y1+2, y2-y1-4);
        ssd1289_drawVLine(x2, y1+2, y2-y1-4);
    }
}

void ssd1289_fillRect(int x1, int y1, int x2, int y2) {
    
    int tmp;

    if (x1 > x2) {
        tmp = x1;
        x1 = x2;
        x2 = tmp;
    }
    
    if (y1 > y2) {
        tmp = y1;
        y1 = y2;
        y2 = tmp;
    }

    if (orient == PORTRAIT) {
        
        for (int i = 0; i < ((y2-y1)/2)+1; i++) {
            ssd1289_drawHLine(x1, y1+i, x2-x1);
            ssd1289_drawHLine(x1, y2-i, x2-x1);
        }
        
    } else { 
        
        for (int i = 0; i < ((x2-x1)/2)+1; i++) {
            ssd1289_drawVLine(x1+i, y1, y2-y1);
            ssd1289_drawVLine(x2-i, y1, y2-y1);
        }
        
    }
}

void ssd1289_fillRoundRect(int x1, int y1, int x2, int y2) {
    
    int tmp;

    if (x1 > x2) {
        tmp = x1;
        x1 = x2;
        x2 = tmp;
    }
    
    if (y1 > y2) {
        tmp = y1;
        y1 = y2;
        y2 = tmp;
    }

    if ((x2-x1) > 4 && (y2-y1) > 4) {
        
        for (int i = 0; i < ((y2-y1)/2)+1; i++) {
            switch(i) {
            case 0:
                ssd1289_drawHLine(x1+2, y1+i, x2-x1-4);
                ssd1289_drawHLine(x1+2, y2-i, x2-x1-4);
                break;
            case 1:
                ssd1289_drawHLine(x1+1, y1+i, x2-x1-2);
                ssd1289_drawHLine(x1+1, y2-i, x2-x1-2);
                break;
            default:
                ssd1289_drawHLine(x1, y1+i, x2-x1);
                ssd1289_drawHLine(x1, y2-i, x2-x1);
            }
        }
    }
}

void ssd1289_drawCircle(int x, int y, int radius) {
    
    int f = 1 - radius;
    int ddF_x = 1;
    int ddF_y = -2 * radius;
    int x1 = 0;
    int y1 = radius;
    char ch, cl;

    ch = ((fcolorr & 248) | fcolorg >> 5);
    cl = ((fcolorg & 28) << 3 | fcolorb >> 3);

    cbi(PORTG, PG1);
    //*P_CS &= ~B_CS;
    
    ssd1289_setXY(x, y + radius, x, y + radius);
    ssd1289_writeData(ch, cl);
    ssd1289_setXY(x, y - radius, x, y - radius);
    ssd1289_writeData(ch, cl);
    ssd1289_setXY(x + radius, y, x + radius, y);
    ssd1289_writeData(ch, cl);
    ssd1289_setXY(x - radius, y, x - radius, y);
    ssd1289_writeData(ch,cl);

    while(x1 < y1) {
        
        if(f >= 0) {
            y1--;
            ddF_y += 2;
            f += ddF_y;
        }
        
        x1++;
        ddF_x += 2;
        f += ddF_x;
        ssd1289_setXY(x + x1, y + y1, x + x1, y + y1);
        ssd1289_writeData(ch,cl);
        ssd1289_setXY(x - x1, y + y1, x - x1, y + y1);
        ssd1289_writeData(ch,cl);
        ssd1289_setXY(x + x1, y - y1, x + x1, y - y1);
        ssd1289_writeData(ch,cl);
        ssd1289_setXY(x - x1, y - y1, x - x1, y - y1);
        ssd1289_writeData(ch,cl);
        ssd1289_setXY(x + y1, y + x1, x + y1, y + x1);
        ssd1289_writeData(ch,cl);
        ssd1289_setXY(x - y1, y + x1, x - y1, y + x1);
        ssd1289_writeData(ch,cl);
        ssd1289_setXY(x + y1, y - x1, x + y1, y - x1);
        ssd1289_writeData(ch,cl);
        ssd1289_setXY(x - y1, y - x1, x - y1, y - x1);
        ssd1289_writeData(ch,cl);
    }
    
    sbi(PORTG, PG1);
    //*P_CS |= B_CS;
    
    if (orient == PORTRAIT)
        ssd1289_setXY(0, 0, 239, disp_y_size);
    else
        ssd1289_setXY(0, 0, disp_y_size, 239);
}

void ssd1289_fillCircle(int x, int y, int radius) {
    
    cbi(PORTG, PG1);
    //*P_CS &= ~B_CS;
    
    for(int y1 = -radius; y1 <= radius; y1++)
        for(int x1 = -radius; x1 <= radius; x1++)
            if(x1*x1+y1*y1 <= radius*radius) {
                ssd1289_setXY(x+x1, y+y1, x+x1, y+y1);
                ssd1289_writeData(((fcolorr&248)|fcolorg>>5),((fcolorg&28)<<3|fcolorb>>3));
            }
    
    sbi(PORTG, PG1);
    //*P_CS |= B_CS;
    
    if (orient == PORTRAIT)
        ssd1289_setXY(0,0,239,disp_y_size);
    else
        ssd1289_setXY(0,0,disp_y_size,239);
}

void ssd1289_clrScr() {
    
    long i;

    cbi(PORTG, PG1);
    //*P_CS &= ~B_CS;
    
    if (orient == PORTRAIT)
        ssd1289_setXY(0, 0, 239, disp_y_size);
    else
        ssd1289_setXY(0, 0, disp_y_size, 239);
    
    sbi(PORTD, PD7);
    //*P_RS |= B_RS;
    
    for (i = 0; i < (240*(disp_y_size+1)); i++) {
        ssd1289_writeBus(0, 0);
    }
    
    sbi(PORTG, PG1);
    //*P_CS |= B_CS;
}

void ssd1289_fillScr(uint8_t r, uint8_t g, uint8_t b) {
    
    long i;
    char ch, cl;

    ch = ((r & 248) | g >> 5);
    cl = ((g & 28) << 3 | b >> 3 );
    
    cbi(PORTG, PG1);
    //*P_CS &= ~B_CS;
    
    if (orient == PORTRAIT)
        ssd1289_setXY(0, 0, 239, disp_y_size);
    else
        ssd1289_setXY(0, 0, disp_y_size, 239);
    
    sbi(PORTD, PD7);    
    //*P_RS |= B_RS;
    
    for (i = 0; i < (240*(disp_y_size+1)); i++) {
        ssd1289_writeBus(ch,cl);
    }
    
    sbi(PORTG, PG1);
    //*P_CS |= B_CS;
}

void ssd1289_setColor(uint8_t r, uint8_t g, uint8_t b) {
    
    fcolorr = r;
    fcolorg = g;
    fcolorb = b;
}

void ssd1289_setBackColor(uint8_t r, uint8_t g, uint8_t b) {
    
    bcolorr = r;
    bcolorg = g;
    bcolorb = b;
}

void ssd1289_setPixel(uint8_t r, uint8_t g, uint8_t b) {
    
    ssd1289_writeData(((r & 248) | g >> 5),((g & 28) << 3 | b >> 3));
}

void ssd1289_drawPixel(int x, int y) {
    cbi(PORTG, PG1);
    //*P_CS &= ~B_CS;
    
    ssd1289_setXY(x, y, x, y);
    ssd1289_setPixel(fcolorr, fcolorg, fcolorb);
    
    sbi(PORTG, PG1);
    //*P_CS |= B_CS;
    
    if (orient == PORTRAIT)
        ssd1289_setXY(0, 0, 239, disp_y_size);
    else
        ssd1289_setXY(0, 0, disp_y_size, 239);
}

void ssd1289_drawLine(int x1, int y1, int x2, int y2) {
    
    int tmp;
    double delta, tx, ty;
    //double m, b, dx, dy;
    char ch, cl;

    ch = ((fcolorr & 248) | fcolorg >> 5);
    cl = ((fcolorg & 28) << 3 | fcolorb >> 3 );

    if (((x2-x1) < 0)) {
        tmp = x1;
        x1 = x2;
        x2 = tmp;
        tmp = y1;
        y1 = y2;
        y2 = tmp;
    }
    
    if (((y2-y1) < 0)) {
        tmp = x1;
        x1 = x2;
        x2 = tmp;
        tmp = y1;
        y1 = y2;
        y2 = tmp;
    }

    if (y1 == y2) {
        if (x1 > x2) {
            tmp = x1;
            x1 = x2;
            x2 = tmp;
        }
        ssd1289_drawHLine(x1, y1, x2-x1);
    } else if (x1 == x2) {
        if (y1 > y2) {
            tmp = y1;
            y1 = y2;
            y2 = tmp;
        }
        ssd1289_drawVLine(x1, y1, y2-y1);
    } else if (abs(x2-x1) > abs(y2-y1)) {
        cbi(PORTG, PG1);
        //*P_CS &= ~B_CS; 
        
        delta = (double) (y2 - y1) / (double) (x2 - x1);
        ty = (double) y1;
        
        if (x1 > x2) {
            for (int i = x1; i >= x2; i--) {
                ssd1289_setXY(i, (int) (ty + 0.5), i, (int) (ty + 0.5));
                ssd1289_writeData(ch, cl);
                ty = ty - delta;
            }
        } else {
            for (int i = x1; i <= x2; i++) {
                ssd1289_setXY(i, (int) (ty + 0.5), i, (int) (ty + 0.5));
                ssd1289_writeData(ch, cl);
                ty = ty + delta;
            }
        }
        
        sbi(PORTG, PG1);
        
    } else {
		cbi(PORTG, PG1);
        //*P_CS &= ~B_CS;
        
        delta = (float) (x2 - x1) / (float) (y2 - y1);
        tx = (float) x1;
        
        if (y1 > y2) {
            for (int i = y2 + 1; i > y1; i--) {
                ssd1289_setXY((int) (tx + 0.5), i, (int) (tx + 0.5), i);
                ssd1289_writeData(ch, cl);
                tx = tx + delta;
            }
        } else {
            for (int i = y1; i < y2 + 1; i++) {
                ssd1289_setXY((int) (tx + 0.5), i, (int) (tx + 0.5), i);
                ssd1289_writeData(ch, cl);
                tx = tx + delta;
            }
        }
        sbi(PORTG, PG1);
        //*P_CS |= B_CS;
    }

    if (orient == PORTRAIT)
        ssd1289_setXY(0, 0, 239, disp_y_size);
    else
        ssd1289_setXY(0, 0, disp_y_size, 239);
}

void ssd1289_drawHLine(int x, int y, int l) {
    
    char ch, cl;

    ch = ((fcolorr & 248) | fcolorg >> 5);
    cl = ((fcolorg & 28) << 3 | fcolorb >> 3);
    
    cbi(PORTG, PG1);
    //*P_CS &= ~B_CS;
    
    ssd1289_setXY(x, y, x+l, y);
    
    for (int i = 0; i < l+1; i++) {
        ssd1289_writeData(ch, cl);
    }
    
    sbi(PORTG, PG1);
    //*P_CS |= B_CS;
}

void ssd1289_drawVLine(int x, int y, int l) {
    
    char ch, cl;

    ch = ((fcolorr & 248) | fcolorg >> 5);
    cl = ((fcolorg & 28) << 3 | fcolorb >> 3);
    
    cbi(PORTG, PG1);
    //*P_CS &= ~B_CS;
    ssd1289_setXY(x, y, x, y+l);
    for (int i=0; i<l; i++) {
        ssd1289_writeData(ch, cl);
    }
    sbi(PORTG, PG1);
    //*P_CS |= B_CS;
}

void ssd1289_printChar(uint8_t c, int x, int y) {
    uint8_t i,j,ch;
    uint32_t temp;
    
    cbi(PORTG, PG1);
   // *P_CS &= ~B_CS;

    if (orient==PORTRAIT) {
        ssd1289_setXY(x,y,x+cfont.x_size-1,y+cfont.y_size-1);

        temp=((c-cfont.offset)*((cfont.x_size/8)*cfont.y_size))+4;
        for(j=0; j<((cfont.x_size/8)*cfont.y_size); j++) {
            ch = pgm_read_byte(&cfont.font[temp]);
            for(i=0; i<8; i++) {
                if((ch&(1<<(7-i)))!=0) {
                    ssd1289_setPixel(fcolorr, fcolorg, fcolorb);
                } else {
                    ssd1289_setPixel(bcolorr, bcolorg, bcolorb);
                }
            }
            temp++;
        }
    } else {
        temp=((c-cfont.offset)*((cfont.x_size/8)*cfont.y_size))+4;

        for(j=0; j<((cfont.x_size/8)*cfont.y_size); j+=(cfont.x_size/8)) {
            ssd1289_setXY(x,y+(j/(cfont.x_size/8)),x+cfont.x_size-1,y+(j/(cfont.x_size/8)));
            for (int zz=(cfont.x_size/8)-1; zz>=0; zz--) {
                ch=pgm_read_byte(&cfont.font[temp+zz]);
                for(i=0; i<8; i++) {
                    if((ch&(1<<i))!=0) {
                        ssd1289_setPixel(fcolorr, fcolorg, fcolorb);
                    } else {
                        ssd1289_setPixel(bcolorr, bcolorg, bcolorb);
                    }
                }
            }
            temp+=(cfont.x_size/8);
        }
    }
    sbi(PORTG, PG1);
    //*P_CS |= B_CS;
}

void ssd1289_rotateChar(uint8_t c, int x, int y, int pos, int deg) {
    uint8_t i,j,ch;
    uint32_t temp;
    int newx,newy;
    double radian;
    radian=deg*0.0175;
    
    cbi(PORTG, PG1);        
    //*P_CS &= ~B_CS;

    temp=((c-cfont.offset)*((cfont.x_size/8)*cfont.y_size))+4;
    for(j=0; j<cfont.y_size; j++) {
        for (int zz=0; zz<(cfont.x_size/8); zz++) {
            ch=pgm_read_byte(&cfont.font[temp+zz]);
            for(i=0; i<8; i++) {
                newx=x+(((i+(zz*8)+(pos*cfont.x_size))*cos(radian))-((j)*sin(radian)));
                newy=y+(((j)*cos(radian))+((i+(zz*8)+(pos*cfont.x_size))*sin(radian)));

                ssd1289_setXY(newx,newy,newx+1,newy+1);

                if((ch&(1<<(7-i)))!=0) {
                    ssd1289_setPixel(fcolorr, fcolorg, fcolorb);
                } else {
                    ssd1289_setPixel(bcolorr, bcolorg, bcolorb);
                }
            }
        }
        temp+=(cfont.x_size/8);
    }
    sbi(PORTG, PG1);
    //*P_CS |= B_CS;
}

void ssd1289_print(char *st, int x, int y, int deg) {
    int stl, i;

    stl = strlen(st);

    if (orient==PORTRAIT) {
        if (x==RIGHT)
            x=240-(stl*cfont.x_size);
        if (x==CENTER)
            x=(240-(stl*cfont.x_size))/2;
    } else {
        if (x==RIGHT)
            x=(disp_y_size+1)-(stl*cfont.x_size);
        if (x==CENTER)
            x=((disp_y_size+1)-(stl*cfont.x_size))/2;
    }

    for (i=0; i<stl; i++)
        if (deg==0)
            ssd1289_printChar(*st++, x + (i*(cfont.x_size)), y);
        else
            ssd1289_rotateChar(*st++, x, y, i, deg);
}

void ssd1289_printNumI(long num, int x, int y) {
    char buf[25];
    char st[27];
    uint8_t neg=false;
    int c=0;

    if (num==0) {
        st[0]=48;
        st[1]=0;
    } else {
        if (num<0) {
            neg=true;
            num=-num;
        }

        while (num>0) {
            buf[c]=48+(num % 10);
            c++;
            num=(num-(num % 10))/10;
        }
        buf[c]=0;

        if (neg) {
            st[0]=45;
        }

        for (int i=0; i<c; i++) {
            st[i+neg]=buf[c-i-1];
        }
        st[c+neg]=0;
    }

    ssd1289_print(st,x,y, 0);
}

void ssd1289_printNumF(double num, uint8_t dec, int x, int y) {
    char buf[25];
    char st[27];
    uint8_t neg=false;
    int c=0;
    int c2;
    unsigned long inum;

    if (num==0) {
        st[0]=48;
        st[1]=46;
        for (int i=0; i<dec; i++)
            st[2+i]=48;
        st[2+dec]=0;
    } else {
        if (num<0) {
            neg=true;
            num=-num;
        }

        if (dec<1)
            dec=1;
        if (dec>5)
            dec=5;

        inum = (long) (num * pow(10, dec));

        while (inum>0) {
            buf[c]=48+(inum % 10);
            c++;
            inum=(inum-(inum % 10))/10;
        }
        if ((num<1) && (num>0)) {
            buf[c]=48;
            c++;
        }
        buf[c]=0;

        if (neg) {
            st[0]=45;
        }

        c2=neg;
        for (int i=0; i<c; i++) {
            st[c2]=buf[c-i-1];
            c2++;
            if ((c-(c2-neg))==dec) {
                st[c2]=46;
                c2++;
            }
        }
        st[c2]=0;
    }

    ssd1289_print(st,x,y, 0);
}

void ssd1289_setFont(const uint8_t* font) {
    cfont.font=font;
    cfont.x_size=pgm_read_byte(&cfont.font[0]);
    cfont.y_size=pgm_read_byte(&cfont.font[1]);
    cfont.offset=pgm_read_byte(&cfont.font[2]);
    cfont.numchars=pgm_read_byte(&cfont.font[3]);
}

void ssd1289_drawBitmap(int x, int y, int sx, int sy, unsigned int* data, int scale) {
    unsigned int col;
    int tx, ty, tc, tsx, tsy;
    //uint8_t r, g, b;

    if (scale==1) {
        if (orient==PORTRAIT) {
			cbi(PORTG, PG1);
            //*P_CS &= ~B_CS;
            ssd1289_setXY(x, y, x+sx-1, y+sy-1);
            for (tc=0; tc<(sx*sy); tc++) {
                col=pgm_read_word(&data[tc]);
                ssd1289_writeData(col>>8,col & 0xff);
            }
            sbi(PORTG, PG1);
           // *P_CS |= B_CS;
        } else {
            cbi(PORTG, PG1);
            //*P_CS &= ~B_CS;
            for (ty=0; ty<sy; ty++) {
                ssd1289_setXY(x, y+ty, x+sx-1, y+ty);
                for (tx=sx; tx>=0; tx--) {
                    col=pgm_read_word(&data[(ty*sx)+tx]);
                    ssd1289_writeData(col>>8,col & 0xff);
                }
            }
            sbi(PORTG, PG1);
            //*P_CS |= B_CS;
        }
    } else {
        if (orient==PORTRAIT) {
            cbi(PORTG, PG1);
            //*P_CS &= ~B_CS;
            for (ty=0; ty<sy; ty++) {
                ssd1289_setXY(x, y+(ty*scale), x+((sx*scale)-1), y+(ty*scale)+scale);
                for (tsy=0; tsy<scale; tsy++)
                    for (tx=0; tx<sx; tx++) {
                        col=pgm_read_word(&data[(ty*sx)+tx]);
                        for (tsx=0; tsx<scale; tsx++)
                            ssd1289_writeData(col>>8,col & 0xff);
                    }
            }
            sbi(PORTG, PG1);
            //*P_CS |= B_CS;
        } else {
            cbi(PORTG, PG1);
            //*P_CS &= ~B_CS;
            for (ty=0; ty<sy; ty++) {
                for (tsy=0; tsy<scale; tsy++) {
                    ssd1289_setXY(x, y+(ty*scale)+tsy, x+((sx*scale)-1), y+(ty*scale)+tsy);
                    for (tx=sx; tx>=0; tx--) {
                        col=pgm_read_word(&data[(ty*sx)+tx]);
                        for (tsx=0; tsx<scale; tsx++)
                            ssd1289_writeData(col>>8,col & 0xff);
                    }
                }
            }
            sbi(PORTG, PG1);
            //*P_CS |= B_CS;
        }
    }
}

void ssd1289_drawBitmapR(int x, int y, int sx, int sy, unsigned int* data, int deg, int rox, int roy) {
    unsigned int col;
    int tx, ty, newx, newy;
    //uint8_t r, g, b;
    double radian;
    radian=deg*0.0175;

    if (deg==0)
        ssd1289_drawBitmap(x, y, sx, sy, data, 1);
    else {
        cbi(PORTG, PG1);
        //*P_CS &= ~B_CS;
        for (ty=0; ty<sy; ty++)
            for (tx=0; tx<sx; tx++) {
                col=pgm_read_byte(&data[(ty*sx)+tx]);
                newx=x+rox+(((tx-rox)*cos(radian))-((ty-roy)*sin(radian)));
                newy=y+roy+(((ty-roy)*cos(radian))+((tx-rox)*sin(radian)));

                ssd1289_setXY(newx, newy, newx, newy);
                ssd1289_writeData(col>>8,col & 0xff);
            }
        sbi(PORTG, PG1);
        //*P_CS |= B_CS;
    }
}

